Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

feat(dynamic_obstacle_stop): check the obstacles behind the EGO #7076

Closed
wants to merge 27 commits into from
Closed

feat(dynamic_obstacle_stop): check the obstacles behind the EGO #7076

wants to merge 27 commits into from

Conversation

beyzanurkaya
Copy link
Contributor

@beyzanurkaya beyzanurkaya commented May 20, 2024 •
edited
Loading

Description

fixes: #7065

Tests performed

Before the PR:

failed_scenario.mp4

After the PR:

success_scenario.mp4

Notes for reviewers

first, merge this: autowarefoundation/autoware_launch#999

Interface changes

ROS Parameter Changes

Parameter Name Default Value Update Description
yaw_threshold 2.35 yaw threshold used for collision checks
yaw_threshold_behind_object 1.0 yaw threshold used for collision checks for objects behind the ego vehicle
ignore_objects_behind_ego false If true, ignore objects that are behind the ego vehicle
behind_object_distance_threshold 5.0 distance behind the ego vehicle to ignore objects
use_predicted_path false if true, use the predicted path of the object to calculate the collision point

Effects on system behavior

Pre-review checklist for the PR author

The PR author must check the checkboxes below when creating the PR.

In-review checklist for the PR reviewers

The PR reviewers must check the checkboxes below before approval.

  • The PR follows the pull request guidelines.
  • The PR has been properly tested.
  • The PR has been reviewed by the code owners.

Post-review checklist for the PR author

The PR author must check the checkboxes below before merging.

  • There are no open discussions or they are tracked via tickets.
  • The PR is ready for merge.

After all checkboxes are checked, anyone who has write access can merge the PR.

@beyzanurkaya beyzanurkaya marked this pull request as draft May 20, 2024 13:57
@beyzanurkaya beyzanurkaya self-assigned this May 20, 2024
@github-actions github-actions bot added the component:planning Route planning, decision-making, and navigation. (auto-assigned) label May 20, 2024
@beyzanurkaya beyzanurkaya added the run:build-and-test-differential Mark to enable build-and-test-differential workflow. (used-by-ci) label May 20, 2024
@beyzanurkaya beyzanurkaya force-pushed the feat/check-behind-objects branch 6 times, most recently from 25b3f61 to 30578bf Compare May 24, 2024 08:35
@beyzanurkaya
Copy link
Contributor Author

@maxime-clem I have completed adding the features but it is not working as I expected. When I use predicted_path for collision check, a collision occurs because this line is empty. I'm investigating this.

autoware.universe/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.cpp

Line 38 in 98cc9f2

boost::geometry::index::intersects(object_footprint), std::back_inserter(rough_collisions));

Screenshot from 2024-05-24 11-40-35
Screenshot from 2024-05-24 11-39-03

@beyzanurkaya beyzanurkaya force-pushed the feat/check-behind-objects branch from b457910 to 4597f75 Compare May 24, 2024 10:44
@beyzanurkaya beyzanurkaya requested a review from brkay54 May 24, 2024 13:26
@beyzanurkaya beyzanurkaya force-pushed the feat/check-behind-objects branch from 129e475 to 1ef2524 Compare May 24, 2024 13:45
@beyzanurkaya beyzanurkaya mentioned this pull request May 24, 2024
Closed
4 tasks
@beyzanurkaya beyzanurkaya force-pushed the feat/check-behind-objects branch 2 times, most recently from 9884d11 to f5d3117 Compare May 24, 2024 14:43
@beyzanurkaya beyzanurkaya marked this pull request as ready for review May 24, 2024 14:50
@beyzanurkaya beyzanurkaya force-pushed the feat/check-behind-objects branch 2 times, most recently from 7fe249e to be17181 Compare May 27, 2024 10:19
maxime-clem
maxime-clem reviewed May 28, 2024
View reviewed changes
Copy link
Contributor

@maxime-clem maxime-clem left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Thank you for improving this module.
I did not test the PR yet but left a few comments about the code.

planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.cpp Outdated Show resolved Hide resolved
planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.cpp Outdated Show resolved Hide resolved
planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp Outdated Show resolved Hide resolved
@beyzanurkaya
Copy link
Contributor Author

beyzanurkaya commented May 28, 2024 •
edited
Loading

@maxime-clem Thank you for your feedback. I tested this PR in a few different scenarios and encountered some problems. As a result, I'm thinking of adding a time_to_collison check because the EGO stops even in a situation that comes from behind at low speed and does not threaten the EGO. What do you think?

2024-05-28.18-32-17.mp4

FYI: @brkay54

@beyzanurkaya beyzanurkaya marked this pull request as draft May 28, 2024 10:37
@beyzanurkaya
Copy link
Contributor Author

@maxime-clem Some changes I made cause behavior_planner_container to die. I haven't found what the problem is yet. Apart from that, I made the changes you requested, but I did not commit them because I could not solve the node dying problem.

[openscenario_interpreter_node-3] [component_container_mt-32] PC: @                0x0 (unknown)
[openscenario_interpreter_node-3] [component_container_mt-32] *** SIGABRT (@0x3e80024a027) received by PID 2400295 (TID 0x768dd6ffd640) from PID 2400295; stack trace: ***
[openscenario_interpreter_node-3] [component_container_mt-32]     @     0x768d79eab006 google::(anonymous namespace)::FailureSignalHandler()
[openscenario_interpreter_node-3] [component_container_mt-32]     @     0x768e06e42520 (unknown)
[openscenario_interpreter_node-3] [component_container_mt-32]     @     0x768e06e969fc pthread_kill
[openscenario_interpreter_node-3] [component_container_mt-32]     @     0x768e06e42476 raise
[openscenario_interpreter_node-3] [component_container_mt-32]     @     0x768e06e287f3 abort
[openscenario_interpreter_node-3] [component_container_mt-32]     @     0x768e072a2b9e (unknown)
[openscenario_interpreter_node-3] [component_container_mt-32]     @     0x768e072ae20c (unknown)
[openscenario_interpreter_node-3] [component_container_mt-32]     @     0x768e072ae277 std::terminate()
[openscenario_interpreter_node-3] [component_container_mt-32]     @     0x768e072ae4d8 __cxa_throw
[openscenario_interpreter_node-3] [component_container_mt-32]     @     0x768e072a27ac (unknown)
[openscenario_interpreter_node-3] [component_container_mt-32]     @     0x768d7cc2800d behavior_velocity_planner::dynamic_obstacle_stop::find_closest_collision_point()
[openscenario_interpreter_node-3] [component_container_mt-32]     @     0x768d7cc2866d behavior_velocity_planner::dynamic_obstacle_stop::find_collisions()
[openscenario_interpreter_node-3] [component_container_mt-32]     @     0x768d7cc47e25 behavior_velocity_planner::dynamic_obstacle_stop::DynamicObstacleStopModule::modifyPathVelocity()
[openscenario_interpreter_node-3] [component_container_mt-32]     @     0x768dcf1b676b behavior_velocity_planner::SceneModuleManagerInterface::modifyPathVelocity()
[openscenario_interpreter_node-3] [component_container_mt-32]     @     0x768dcf58a7df autoware::behavior_velocity_planner::BehaviorVelocityPlannerManager::planPathVelocity()
[openscenario_interpreter_node-3] [component_container_mt-32]     @     0x768dcf4a7f08 autoware::behavior_velocity_planner::BehaviorVelocityPlannerNode::generatePath()
[openscenario_interpreter_node-3] [component_container_mt-32]     @     0x768dcf4a868a autoware::behavior_velocity_planner::BehaviorVelocityPlannerNode::onTrigger()
[openscenario_interpreter_node-3] [component_container_mt-32]     @     0x768dcf4e6c07 std::_Function_handler<>::_M_invoke()
[openscenario_interpreter_node-3] [component_container_mt-32]     @     0x768dcf4e2e42 _ZNSt8__detail9__variant17__gen_vtable_implINS0_12_Multi_arrayIPFNS0_21__deduce_visit_resultIvEEOZN6rclcpp23AnySubscriptionCallbackIN27autoware_auto_planning_msgs3msg15PathWithLaneId_ISaIvEEESA_E8dispatchESt10shared_ptrISB_ERKNS5_11MessageInfoEEUlOT_E_RSt7variantIJSt8functionIFvRKSB_EESN_IFvSP_SH_EESN_IFvRKNS5_17SerializedMessageEEESN_IFvSW_SH_EESN_IFvSt10unique_ptrISB_St14default_deleteISB_EEEESN_IFvS14_SH_EESN_IFvS11_ISU_S12_ISU_EEEESN_IFvS1A_SH_EESN_IFvSD_ISO_EEESN_IFvS1F_SH_EESN_IFvSD_ISV_EEESN_IFvS1K_SH_EESN_IFvRKS1F_EESN_IFvS1Q_SH_EESN_IFvRKS1K_EESN_IFvS1W_SH_EESN_IFvSE_EESN_IFvSE_SH_EESN_IFvSD_ISU_EEESN_IFvS25_SH_EEEEEJEEESt16integer_sequenceImJLm8EEEE14__visit_invokeESL_S2B_
[openscenario_interpreter_node-3] [component_container_mt-32]     @     0x768dcf575d9f rclcpp::Subscription<>::handle_message()
[openscenario_interpreter_node-3] [component_container_mt-32]     @     0x768e076227bc rclcpp::Executor::execute_subscription()
[openscenario_interpreter_node-3] [component_container_mt-32]     @     0x768e07622fbf rclcpp::Executor::execute_any_executable()
[openscenario_interpreter_node-3] [component_container_mt-32]     @     0x768e0762a27a rclcpp::executors::MultiThreadedExecutor::run()
[openscenario_interpreter_node-3] [component_container_mt-32]     @     0x768e072dc253 (unknown)
[openscenario_interpreter_node-3] [component_container_mt-32]     @     0x768e06e94ac3 (unknown)
[openscenario_interpreter_node-3] [component_container_mt-32]     @     0x768e06f26850 (unknown)

@beyzanurkaya
Copy link
Contributor Author

I solved the problem of stopping even for slow objects with a simple time_to_collison check.

2024-06-04.10-23-51.mp4

@github-actions github-actions bot added the component:control Vehicle control algorithms and mechanisms. (auto-assigned) label Jun 4, 2024
@beyzanurkaya beyzanurkaya force-pushed the feat/check-behind-objects branch from bf5784b to c43377c Compare June 4, 2024 14:58
@beyzanurkaya beyzanurkaya marked this pull request as ready for review June 5, 2024 09:17
beyzanurkaya and others added 24 commits June 5, 2024 17:04
@beyzanurkaya
update function's arguments
59ca0d0 
Signed-off-by: beyza <[email protected]>
@beyzanurkaya
get variables
7f9684d 
Signed-off-by: beyza <[email protected]>
@beyzanurkaya
add necessary parameters
438d808 
Signed-off-by: beyza <[email protected]>
@pre-commit-ci @beyzanurkaya
style(pre-commit): autofix
4bd8d4e
@beyzanurkaya
add&get use_predicted_path
f676487 
Signed-off-by: beyza <[email protected]>
@beyzanurkaya
create object's predicted_footprint
12e18ff 
Signed-off-by: beyza <[email protected]>
@beyzanurkaya
add an option for collision check
585fe42 
Signed-off-by: beyza <[email protected]>
@pre-commit-ci @beyzanurkaya
style(pre-commit): autofix
eaaedeb
@beyzanurkaya
add extra_object_footprint_width for predicted_footprint
f2cc98f 
Signed-off-by: beyza <[email protected]>
@beyzanurkaya
update filtering condition for behind object
f72fb2b 
Signed-off-by: beyza <[email protected]>
@beyzanurkaya
update collision check for multipolygon
74e3835 
Signed-off-by: beyza <[email protected]>
@pre-commit-ci @beyzanurkaya
style(pre-commit): autofix
b3a79a7
@beyzanurkaya
fix cpplint error
60d42b6 
Signed-off-by: beyza <[email protected]>
@pre-commit-ci @beyzanurkaya
style(pre-commit): autofix
089785b
@beyzanurkaya
object filtering
246a0e9 
Signed-off-by: beyza <[email protected]>
@beyzanurkaya
change object footprint type
21a411b 
Signed-off-by: beyza <[email protected]>
@beyzanurkaya
update return condition
9392a6c 
Signed-off-by: beyza <[email protected]>
@beyzanurkaya
change function arguments
dcda7f5 
Signed-off-by: beyza <[email protected]>
@beyzanurkaya
simplify collision check
6866bb3 
Signed-off-by: beyza <[email protected]>
@beyzanurkaya
add collison time
6fed02d 
Signed-off-by: beyza <[email protected]>
@pre-commit-ci @beyzanurkaya
style(pre-commit): autofix
9954717
@beyzanurkaya
update dist_obj_to_coll
f095639 
Signed-off-by: beyza <[email protected]>
@pre-commit-ci @beyzanurkaya
style(pre-commit): autofix
501b643
@beyzanurkaya
update msg type
f351fb6 
Signed-off-by: beyza <[email protected]>
@beyzanurkaya beyzanurkaya force-pushed the feat/check-behind-objects branch from 7761f70 to f351fb6 Compare June 5, 2024 14:04
@github-actions github-actions bot removed the component:control Vehicle control algorithms and mechanisms. (auto-assigned) label Jun 5, 2024
@stale Stale
Copy link

stale bot commented Aug 4, 2024

This pull request has been automatically marked as stale because it has not had recent activity.

@stale stale bot added the status:stale Inactive or outdated issues. (auto-assigned) label Aug 4, 2024
@beyzanurkaya
Copy link
Contributor Author

@brkay54 and I discussed behind objects and decided to handle such objects in obstacle_cruise_planner. So I'm closing this PR.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Reviewers

@soblin soblin Awaiting requested review from soblin

@brkay54 brkay54 Awaiting requested review from brkay54

@danielsanchezaran danielsanchezaran Awaiting requested review from danielsanchezaran

@TakaHoribe TakaHoribe Awaiting requested review from TakaHoribe

@tkimura4 tkimura4 Awaiting requested review from tkimura4

@maxime-clem maxime-clem Awaiting requested review from maxime-clem

Assignees

@beyzanurkaya beyzanurkaya

Labels
component:planning Route planning, decision-making, and navigation. (auto-assigned) run:build-and-test-differential Mark to enable build-and-test-differential workflow. (used-by-ci) status:stale Inactive or outdated issues. (auto-assigned)
Projects
None yet
Milestone
No milestone
Development

Successfully merging this pull request may close these issues.

dynamic_obstacle_stop_module does not check obstacles behind EGO's vehicle
2 participants
@beyzanurkaya @maxime-clem